
package edu.wpi.first.wpilibj.templates.subsystems;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.*;

/**
 * @author Eric
 */

public class ShooterSystem extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    Victor m_winch;
    Solenoid m_dogEngage;
    DigitalInput m_lowerLimit;
    DigitalInput m_upperLimit;
    
    boolean[] m_status;
    
    boolean m_running;
    
    public void ShooterSystem(){
        m_winch = new Victor(RobotMap.winchmotor);
        m_dogEngage = new Solenoid(RobotMap.winchrelease);
        m_lowerLimit = new DigitalInput(RobotMap.shooterposdetectbegin);
        m_upperLimit = new DigitalInput(RobotMap.shooterposdetectfinish);
        
        m_status = new boolean[2];
        m_status[0] = m_lowerLimit.get();
        m_status[1] = m_upperLimit.get();
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    
    public boolean[] getStatus(){
        return m_status;
    }
    
    public void updateSensors(){
        m_status[0] = m_lowerLimit.get();
        m_status[1] = m_upperLimit.get();
    }
    
    public void setDog( boolean activation){
        m_dogEngage.set(activation);
    }
    
    public void setWinch( boolean activation){
        if (activation){
            startWinch();
        }
        else {
            stopWinch();
        }
    }
    
    private void stopWinch(){
        m_running = false;
        m_winch.set(0.0);
    }
    
    private void startWinch(){
        m_running = true;
        m_winch.set(1.0);
    }
}
